import java.util.LinkedList;
import java.util.List;

public class PhysicsObject {
	
	SceneGraphNode renderable;

	public static final float GRAVITY = -9.8f;
	public static final float SIN_60 = (float)Math.sin(Math.PI / 3);
	
	/** Collision depths less than this are ignored. **/
	public static final float EPSILON = 1E-7f;
	
	public static float elasticity = 1.7f;
	
	protected Vector2f position = new Vector2f();
	protected Vector2f velocity = new Vector2f();
	protected Vector2f acceleration = new Vector2f();
	protected Vector2f centerOfMass = new Vector2f();
	protected float orientation;
	protected float inverseMass = 1;
	 // inverseMomentOfInertia needs to be set in subclasses
	protected float inverseMomentOfInertia;
	protected float angularVelocity;
	
	protected Vector2f positionRelativeToParent = new Vector2f();
	protected float orientationRelativeToParent;
	protected List<PhysicsObject> children = new LinkedList<PhysicsObject>();
	
	public void addChild(PhysicsObject node) {
		if (node == this)
			throw new IllegalArgumentException();
		children.add(node);
	}
	
	public void removeChild(PhysicsObject node) {
		children.remove(node);
	}

	public void updateState(float timePeriod) {
		position.sumScale(velocity, timePeriod);
		position.sumScale(acceleration, timePeriod * timePeriod / 2);
		velocity.sumScale(acceleration, timePeriod);
		orientation += angularVelocity * timePeriod;
		clearCaches();
		updateChildrenState();
	}
	
	public void updateChildrenState() {
		for (PhysicsObject child : children) {
			child.position.x = position.x + child.positionRelativeToParent.x;
			child.position.y = position.y + child.positionRelativeToParent.y;
			child.orientation = orientation + orientationRelativeToParent;
			child.clearCaches();
		}
	}

	public void updateRenderable() {
		if (renderable != null) {
			renderable.translateX = position.x;
			renderable.translateY = position.y;
			renderable.rotation = (float) (180 * orientation / Math.PI);
		}
		for (PhysicsObject child : children) child.updateRenderable();
	}

	public void clearCaches() {
	}

	public final CollisionInfo getCollision(PhysicsObject other) {
		if (inverseMass == 0 && other.inverseMass == 0)
			return null;
		
		//HalfSpace vs. Square
		if (this instanceof HalfSpace && other instanceof Square)
			return Square.getCollision((HalfSpace) this, (Square) other);

		if (this instanceof Square && other instanceof HalfSpace) {
			CollisionInfo cInfo = Square.getCollision((HalfSpace) other, (Square) this);
			if (cInfo != null)
				cInfo.flip();
			return cInfo;
		}
		
		//Triangle vs. HalfSpace
		if (this instanceof HalfSpace && other instanceof Triangle)
			return Triangle.getCollision((HalfSpace) this, (Triangle) other);

		if (this instanceof Triangle && other instanceof HalfSpace) {
			CollisionInfo cInfo = Triangle.getCollision((HalfSpace) other, (Triangle) this);
			if (cInfo != null)
				cInfo.flip();
			return cInfo;
		}
		
		//Circle vs. Triangle
		if (this instanceof Circle && other instanceof Triangle)
			return Triangle.getCollision((Circle) this, (Triangle) other);
		if (this instanceof Triangle && other instanceof Circle) {
			CollisionInfo cInfo = Triangle.getCollision((Circle) other, (Triangle) this);
			if (cInfo != null)
				cInfo.flip();
			return cInfo;
		}
		
		//Triangle vs. Triangle
		if (this instanceof Triangle && other instanceof Triangle)
			return Triangle.getCollision((Triangle) other, (Triangle) this);
		
		//Circle vs. HalfSpace
		if (this instanceof HalfSpace && other instanceof Circle)
			return getCollision((HalfSpace) this, (Circle) other);
		if (this instanceof Circle && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace) other, (Circle) this);
			if (cInfo != null)
				cInfo.flip();
			return cInfo;
		}
		
		//Rectangle vs. HalfSpace
		if (this instanceof HalfSpace && other instanceof Rectangle)
			return ((Rectangle)other).getRectangleCollision((HalfSpace)this);

		if (this instanceof Rectangle && other instanceof HalfSpace) {
			CollisionInfo cInfo = ((Rectangle)this).getRectangleCollision((HalfSpace)other);
			if (cInfo != null)
				cInfo.flip();
			return cInfo;
		}
		
		//Circle vs. Circle
		if (this instanceof Circle && other instanceof Circle)
			return getCollision((Circle) this, (Circle) other);
		
		return null;
	}

	private static CollisionInfo getCollision(HalfSpace a, Circle b) {
		float distance = a.normal.dot(b.position);
		distance -= a.intercept;
		if (distance >= b.radius)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = b.radius - distance;
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(b.position);
		cInfo.positionA.sumScale(cInfo.normal, -b.radius + cInfo.depth);
		cInfo.positionB = new Vector2f(b.position);
		cInfo.positionB.sumScale(cInfo.normal, -b.radius);
		return cInfo;
	}

	private static CollisionInfo getCollision(Circle a, Circle b) {
		Vector2f tmp = new Vector2f(b.position);
		tmp.sumScale(a.position, -1);
		float distance = tmp.length() - a.radius - b.radius;
		if (distance >= 0)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distance;
		tmp.normalize();
		cInfo.normal = tmp;
		cInfo.positionA = new Vector2f(a.position);
		cInfo.positionA.sumScale(cInfo.normal, a.radius);
		cInfo.positionB = new Vector2f(b.position);
		cInfo.positionB.sumScale(cInfo.normal, -b.radius);
		return cInfo;
	}
	
	public void resolveCollision(PhysicsObject other, CollisionInfo cInfo) {
		if (cInfo.depth > EPSILON) {
			// Calculate the velocity of the collision point on the calling object.
			Vector2f relativeCollisionPositionA = new Vector2f(cInfo.positionA);
			relativeCollisionPositionA.sumScale(position, -1);
			relativeCollisionPositionA.sumScale(centerOfMass, -1);
			Vector2f linearVelocityA = new Vector2f(
					-relativeCollisionPositionA.y, relativeCollisionPositionA.x);
			linearVelocityA.scale(angularVelocity);
			linearVelocityA.sum(velocity);

			// Calculate the velocity of the collision point on the other object.
			Vector2f relativeCollisionPositionB = new Vector2f(cInfo.positionB);
			relativeCollisionPositionB.sumScale(other.position, -1);
			relativeCollisionPositionB.sumScale(other.centerOfMass, -1);
			Vector2f linearVelocityB = new Vector2f(
					-relativeCollisionPositionB.y, relativeCollisionPositionB.x);
			linearVelocityB.scale(other.angularVelocity);
			linearVelocityB.sum(other.velocity);

			// Calculate the relative velocity between the calling object and
			// other object, as if the calling object were stationary and only
			// the other object were moving.
			Vector2f relativeVelocity = new Vector2f(linearVelocityB);
			relativeVelocity.sumScale(linearVelocityA, -1);

			// Calculate the component of the relative velocity that lays along
			// the collision normal.
			float compRelVelAlongNormal = relativeVelocity.dot(cInfo.normal);

			// Calculate the resulting impulse per unit mass.
			float impulse = (float) (elasticity * compRelVelAlongNormal / (inverseMass
					+ other.inverseMass + Math.pow(relativeCollisionPositionA.cross(cInfo.normal), 2)
						* inverseMomentOfInertia + Math.pow( relativeCollisionPositionB.cross(cInfo.normal), 2)
						* other.inverseMomentOfInertia));

			// Adjust the linear and angular velocities of each object in
			// proportion to their effective masses.
			velocity.sumScale(cInfo.normal, impulse * inverseMass);
			other.velocity.sumScale(cInfo.normal, -impulse * other.inverseMass);
			angularVelocity += relativeCollisionPositionA.cross(cInfo.normal)
					* impulse * inverseMomentOfInertia;
			other.angularVelocity -= relativeCollisionPositionB.cross(cInfo.normal)
					* impulse * other.inverseMomentOfInertia;

			// Calculate the amount of object overlap per unit mass.
			float depth = (float) (cInfo.depth / (inverseMass
					+ other.inverseMass
					+ Math.pow(relativeCollisionPositionA.cross(cInfo.normal), 2)
						* inverseMomentOfInertia + Math.pow(relativeCollisionPositionB.cross(cInfo.normal), 2)
						* other.inverseMomentOfInertia));

			// Adjust the position and orientation of each object in proportion
			// to their effective masses to remove overlap.
			position.sumScale(cInfo.normal, -depth * inverseMass);
			other.position.sumScale(cInfo.normal, depth * other.inverseMass);
			orientation -= relativeCollisionPositionA.cross(cInfo.normal)
					* depth * inverseMomentOfInertia;
			other.orientation += relativeCollisionPositionB.cross(cInfo.normal)
					* depth * other.inverseMomentOfInertia;
			clearCaches();
			other.clearCaches();
			updateChildrenState();
			other.updateChildrenState();
			for (PhysicsObject child : children) child.clearCaches();
			for (PhysicsObject child : other.children) child.clearCaches();
		}
	}
}
